
#include "../include/referenceline_provider.hpp"

ReferenceLineProvider::ReferenceLineProvider() {
    // ros::NodeHandle private_node("~");
    // 加载参数
    ros::param::get("rear_point_num", this->rear_point_num);
    ros::param::get("front_point_num", this->front_point_num);
    ros::param::get("need_smooth", this->need_smooth);

    m_index_param_name = "m_index_global";
    if (!ros::param::has(m_index_param_name)) {
        ros::param::set(m_index_param_name, this->m_index);
    } else {
        ros::param::get(m_index_param_name, m_index);
    }

    this->qp_solver = new QPSolver(false);
    this->w_smooth = 5.0;
    this->w_length = 1.0;
    this->w_ref = 1.0;
    this->lb = -0.5;
    this->ub = 0.5;
}

ReferenceLineProvider::~ReferenceLineProvider() {

}

void ReferenceLineProvider::calculateHeading(double x1, double y1, double x2, double y2, double &theta) {
	double xx, yy;
	xx = x2 - x1;
	yy = y2 - y1;
	if (xx == 0.0)
		theta = PI / 2.0;
	else
		theta = atan(fabs(yy / xx));
	if ((xx < 0.0) && (yy >= 0.0))
		theta = PI - theta;
	else if ((xx < 0.0) && (yy < 0.0))
		theta = PI + theta;
	else if ((xx >= 0.0) && (yy < 0.0))
		theta = PI * 2.0 - theta;
}


void ReferenceLineProvider::calculateHeadingAndS(vector<ReferencePoint> &path) {
    double accumulated_s = 0;       // 弧长
    int i = 0;
    for (; i < path.size() - 1; i++) {
        double heading;
        this->calculateHeading(path[i].x, path[i].y, path[i+1].x, path[i+1].y, heading);
        double dis = sqrt(pow(path[i].x - path[i+1].x, 2) + pow(path[i].y - path[i+1].y, 2));
        path[i].heading = heading;
        // path[i].kappa = path[i].heading / dis;
        path[i].s = accumulated_s;
        accumulated_s += dis;
    }   
    // 因为设计的道路是首尾相连的，这里计算最后一个点的 heading kappa s
    // 对于 path[i] 的后一个点就是 path[0]
    double h_;
    this->calculateHeading(path[i].x, path[i].y, path[0].x, path[0].y, h_);
    double distance = sqrt(pow(path[i].x - path[0].x, 2) + pow(path[i].y - path[0].y, 2));
    path[i].heading = h_;
    path[i].s = accumulated_s;
}


void ReferenceLineProvider::calculateKappaAndDKappa(vector<ReferencePoint> &path) {
    int len = path.size();
    // 首先处理第一个点
    ReferencePoint nxt_p = path[1];         // 第一个点的后一个点
    ReferencePoint pre_p = path[len-1];     // 第一个点的前一个点
    double d_heading = abs(nxt_p.heading - pre_p.heading);
    double d_dis = sqrt(pow(pre_p.x - nxt_p.x, 2) + pow(pre_p.y - nxt_p.y, 2));
    path[0].kappa = d_heading / d_dis;
    path[0].dkappa = d_heading / pow(d_dis, 2);

    // 处理最后一个点
    pre_p = path[len - 2];                  // 最后一个点的前一个点（倒数第二个）
    nxt_p = path[0];                        // 最后一个点的下一个点(第一个点)
    d_heading = abs(nxt_p.heading - pre_p.heading);
    d_dis = sqrt(pow(pre_p.x - nxt_p.x, 2) + pow(pre_p.y - nxt_p.y, 2));
    path[len - 1].kappa = d_heading / d_dis;
    path[len - 1].dkappa = d_heading / pow(d_dis, 2);
    
    for (int i = 1; i < path.size() - 1; i++) {
        pre_p = path[i - 1];
        nxt_p = path[i + 1];
        d_heading = abs(nxt_p.heading - pre_p.heading);
        d_dis = sqrt(pow(pre_p.x - nxt_p.x, 2) + pow(pre_p.y - nxt_p.y, 2));
        path[i].kappa = d_heading / d_dis;
        path[i].dkappa = d_heading / pow(d_dis, 2);
    }
}


bool ReferenceLineProvider::calculateProjection(const double x, const double y, 
                                                const vector<ReferencePoint> &path, int &match_index, 
                                                ReferencePoint &proj_point, int pre_match_index) {
    Vector2f car_posi(x, y);
    /// 1. 寻找匹配点
    double pre_dis = 1e8;
    int i = pre_match_index;
    for (; i < path.size(); i++) {
        Vector2f tmp_posi(path.at(i).x, path.at(i).y);
        double cur_dis = (car_posi - tmp_posi).norm();
        if (cur_dis < pre_dis) {
            pre_dis = cur_dis;
        } else {
            match_index = i - 1;
            break;
        }
    }
    if (i == path.size() - 1) {
        ROS_WARN_ONCE("calculateProjection: can't find match point");
        return false;
    }
    // 为了方便调试，当小车到终点后，把匹配点设置为全局路径起点
    if (this->global_path_length_ - match_index < 5 && this->global_path_length_ != 0) {
        match_index = 0;
    }
    /// 2. 计算投影
    // 拿出匹配点信息
    double m_heading = path.at(match_index).heading;
    double m_kappa = path.at(match_index).kappa;
    double m_dkappa = path.at(match_index).dkappa;
    Vector2f m_unit_vec(cos(m_heading), sin(m_heading));
    Vector2f m_vec(path.at(match_index).x, path.at(match_index).y);

    Vector2f d_ved = car_posi - m_vec;
    // 计算投影点坐标向量 和 heading
    Vector2f r_vec = m_vec + d_ved.dot(m_unit_vec) * m_unit_vec;
    double r_heading = m_heading + m_kappa * d_ved.dot(m_unit_vec);
    proj_point.x = r_vec[0];
    proj_point.y = r_vec[1];
    proj_point.heading = r_heading;
    // kappa 和 dkappa 就使用匹配点的信息
    proj_point.kappa = m_kappa;
    proj_point.dkappa = m_dkappa;
    ReferencePoint match_point = path[match_index];
    double dis = sqrt(pow(proj_point.x - match_point.x, 2) + pow(proj_point.y - match_point.y, 2));
    proj_point.s = match_point.s + dis / 2.0;
    return true;
}


bool ReferenceLineProvider::setProjPointAsBegin(const int match_index, 
                                                const ReferencePoint &proj_point, 
                                                vector<ReferencePoint> &cur_ref_line) 
{
    if (match_index >= cur_ref_line.size()) {
        ROS_WARN_ONCE("setProjPointAsBegin: match_index is larger than reference_line size");
        return false;
    }
    double m_s = cur_ref_line[match_index].s;
    double m_x = cur_ref_line[match_index].x;
    double m_y = cur_ref_line[match_index].y;
    // 计算匹配点到投影点的长度
    double delta_s = sqrt(pow(m_x - proj_point.x, 2) + pow(m_y - proj_point.y, 2));
    // 投影点到匹配点的向量
    Vector2d d_vec(proj_point.x - m_x, proj_point.y - m_y);       
    // 匹配点的法向量
    Vector2d m_heading_vec(cos(cur_ref_line[match_index].heading), sin(cur_ref_line[match_index].heading));
    double offset_s;
    if (d_vec.dot(m_heading_vec) <= 0) {
        // 投影点在匹配点前面
        offset_s = m_s + delta_s;
    } else {
        // 投影点在匹配点后方
        offset_s = m_s - delta_s;
    }
    // 将参考线的原点设置为 投影点
    for (int i = 0; i < cur_ref_line.size(); i++) {
        cur_ref_line[i].s -= offset_s;
    }
    return true;
}


void ReferenceLineProvider::global_path_callback(const nav_msgs::Path &msg) {
    
    this->global_path_length_ = msg.poses.size();
    vector<ReferencePoint> path;
    for (int i = 0; i < msg.poses.size(); i++) {
        ReferencePoint point;
        point.x = msg.poses[i].pose.position.x;
        point.y = msg.poses[i].pose.position.y;
        path.push_back(point);
    }
    // 计算 heading and s
    this->calculateHeadingAndS(path);
    // 计算 kappa 和 dkappa
    this->calculateKappaAndDKappa(path);

    global_path_mutex_.lock();
    this->global_path_vec.clear();
    this->global_path_vec.assign(path.begin(), path.end());
    global_path_mutex_.unlock();
}


void ReferenceLineProvider::rear_pose_callback(const geometry_msgs::PoseStamped &msg) {
    this->car_x = msg.pose.position.x;
    this->car_y = msg.pose.position.y;
}

// 发布reference line 话题函数
void ReferenceLineProvider::pubReferenceLine() {

    ros::Rate loop(10);
    
    while (ros::ok()) {
        if (this->global_path_vec.size() == 0)
            continue;
        global_path_mutex_.lock();  // 对 global_path_vec 数据进行上锁

        /**
         * 步骤1. 获取当前车辆在global_path上的投影点，并更新 this->m_index
        */
        ReferencePoint proj_point;
        int match_index;
        if (calculateProjection(car_x, car_y, this->global_path_vec, match_index, proj_point, this->m_index)) {
            this->m_index = match_index;        // 记录当前匹配点
            ros::param::set(m_index_param_name, m_index);
        } else {
            continue;
        }

        /**
         * 步骤2. 根据 match_index 提取一段路径作为 reference_line
        */
        int begin_offset;           // 开始截取的位置
        if (match_index + this->front_point_num > this->global_path_vec.size()) {
            begin_offset = this->global_path_vec.size() - rear_point_num - front_point_num;
        } else if (match_index < rear_point_num) {     // 当车辆后面距离不够时
            begin_offset = 0;
        } else {
            begin_offset = match_index - rear_point_num;
        }
        vector<ReferencePoint>::const_iterator begin = global_path_vec.begin() + begin_offset;
        vector<ReferencePoint> reference_line;
        reference_line.assign(begin, begin + rear_point_num + front_point_num);
        global_path_mutex_.unlock();
        // cout << "match index: " << match_index << " Reference_line: " << reference_line.size() << endl;
        /**
         * 步骤3：对reference_line的弧长s进行处理，使投影点作为参考线坐标原点
         * match_index 表示在投影点在 global_path 中的下标，
         * 那么在 reference_line 中的下标 为 match_index - begin_offset
        */
        this->setProjPointAsBegin(match_index - begin_offset, proj_point, reference_line);

        /**
         * 步骤4. 拿到 reference_line 后，开始进行平滑处理，使用FEM Smooth算法进行优化
        */
        if (this->need_smooth) {
            // 将要优化的参考线给优化类
            this->qp_solver->updateRefePoint(reference_line);
            // 初始化优化类的矩阵
            this->qp_solver->initFEMSmoothMat(reference_line.size(), w_smooth, w_length, w_ref, lb, ub);
            vector<Point> smooth_point;
            if (!this->qp_solver->run(smooth_point) || smooth_point.size() != reference_line.size()) {
                ROS_WARN_ONCE("OSQP meet some wrong !!!");
            } else {
                for (int i = 0; i < smooth_point.size(); i++) {
                    reference_line[i].x = smooth_point[i].x;
                    reference_line[i].y = smooth_point[i].y;
                }
            }
        }
        
        /**
         * 步骤5：平滑处理后，发布为 nav_msgs/Path 消息
        */
        nav_msgs::Path reference_line_path;
        reference_line_path.header.frame_id = "world";
        reference_line_path.header.stamp = ros::Time::now();
        reference_line_path.poses.clear();
        for (int i = 0; i < reference_line.size(); i++) {
            geometry_msgs::PoseStamped pose;
            pose.header.frame_id = "world";
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = reference_line[i].x;
            pose.pose.position.y = reference_line[i].y;
            pose.pose.position.z = 0;

            pose.pose.orientation.x = 0.0;
            pose.pose.orientation.y = 0.0;
            pose.pose.orientation.z = 0.0;
            pose.pose.orientation.w = 0.0;
            reference_line_path.poses.push_back(pose);
        }
        this->reference_line_pub_.publish(reference_line_path);

        loop.sleep();

        reference_line_mutex_.lock();
        this->reference_line_smooth.clear();
        this->reference_line_smooth.assign(reference_line.begin(), reference_line.end());
        reference_line_mutex_.unlock();
    }
}

/**
 * 全局坐标转自然坐标系
 * @param proj_point: 投影点信息
 * @param host: 笛卡尔坐标下的小车坐标
 * @param sl_point: 要求的 SL 坐标信息
*/
void ReferenceLineProvider::cartesian2Frenet(const ReferencePoint &proj_point, 
                                             const Location &host, SLPoint &sl_point) {
    const double rs = proj_point.s;
    const double rx = proj_point.x;
    const double ry = proj_point.y;
    const double rtheta = proj_point.heading;
    const double rkappa = proj_point.kappa;
    const double rdkappa = proj_point.dkappa;
    const double x = host.x;
    const double y = host.y;
    const double v = host.speed;
    const double a = host.accel;
    const double theta = host.heading;
    const double kappa = host.kappa;

    const double dx = x - rx;
    const double dy = y - ry;

    const double cos_theta_r = std::cos(rtheta);
    const double sin_theta_r = std::sin(rtheta);
        // 这里变量名应该为点乘 dot_rd_nd
    const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;
    sl_point.l = std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

    const double delta_theta = theta - rtheta;
    const double tan_delta_theta = std::tan(delta_theta);
    const double cos_delta_theta = std::cos(delta_theta);

    const double one_minus_kappa_r_d = 1 - rkappa * sl_point.l;
    sl_point.l_prime = one_minus_kappa_r_d * tan_delta_theta;

    const double kappa_r_d_prime =
        rdkappa * sl_point.l + rkappa * sl_point.l_prime;

    sl_point.l_dprime =
        -kappa_r_d_prime * tan_delta_theta +
        one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *
            (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);

    sl_point.s = rs;

    sl_point.s_dot = v * cos_delta_theta / one_minus_kappa_r_d;

    const double delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;
    sl_point.s_ddot = (a * cos_delta_theta - sl_point.s_dot * sl_point.s_dot *
            (sl_point.l_prime * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d;
    return;
}

/**
 * 自然坐标转笛卡尔坐标
 * @param proj_point: 投影点信息
 * @param sl_point: SL坐标点
 * @param host: 要求的笛卡尔坐标
*/
void ReferenceLineProvider::frenet2Cartesian(const ReferencePoint &proj_point, 
                                             const SLPoint &sl_point, Location &host) {
    const double rs = proj_point.s;
    const double rx = proj_point.x;
    const double ry = proj_point.y;
    const double rtheta = proj_point.heading;
    const double rkappa = proj_point.kappa;
    const double rdkappa = proj_point.dkappa;

    // if (std::abs(rs - sl_point.s) < 1.0e-6)
    //     std::cout << "The reference point s and sl_point.s don't match" << std::endl;

    const double cos_theta_r = std::cos(rtheta);
    const double sin_theta_r = std::sin(rtheta);

    host.x = rx - sin_theta_r * sl_point.l;
    host.y = ry + cos_theta_r * sl_point.l;

    const double one_minus_kappa_r_d = 1 - rkappa * sl_point.l;

    const double tan_delta_theta = sl_point.l_prime / one_minus_kappa_r_d;
    const double delta_theta = std::atan2(sl_point.l_prime, one_minus_kappa_r_d);
    const double cos_delta_theta = std::cos(delta_theta);

    host.heading = NormalizeAngle(delta_theta + rtheta);

    const double kappa_r_d_prime = rdkappa * sl_point.l + rkappa * sl_point.l_prime;

    host.kappa = (((sl_point.l_dprime + kappa_r_d_prime * tan_delta_theta) *
                    cos_delta_theta * cos_delta_theta) / (one_minus_kappa_r_d) + rkappa) * 
                    cos_delta_theta / (one_minus_kappa_r_d);

    const double d_dot = sl_point.l_prime * sl_point.s_dot;
    host.speed = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *
                            sl_point.s_dot * sl_point.s_dot + d_dot * d_dot);

    const double delta_theta_prime =
        one_minus_kappa_r_d / cos_delta_theta * (host.kappa) - rkappa;

    host.accel = sl_point.s_ddot * one_minus_kappa_r_d / cos_delta_theta +
            sl_point.s_dot * sl_point.s_dot / cos_delta_theta *
                (sl_point.l_prime * delta_theta_prime - kappa_r_d_prime);
    return;
}


double ReferenceLineProvider::NormalizeAngle(const double angle) {
    double a = std::fmod(angle + PI, 2.0 * PI);
    if (a < 0.0) {
        a += (2.0 * PI);
    }
    return a - PI;
}


bool ReferenceLineProvider::XY2SL(const Location &xy_point, 
                                  const vector<ReferencePoint> &path, 
                                  SLPoint &sl_point) {
    int m_index;
    ReferencePoint proj_point;
    if (!this->calculateProjection(xy_point.x, xy_point.y, path, m_index, proj_point))
        return false;
    this->cartesian2Frenet(proj_point, xy_point, sl_point);
    return true;
}


vector<ReferencePoint> ReferenceLineProvider::getReferenceLine() {
    // 规划模块需要密集的点，所以需要对参考线进行插值增密
    // global_path 两点间隔为 1 米，这里对增密到 0.5 米
    vector<ReferencePoint> ref_final;
    if (this->reference_line_smooth.size() != 0) {
        this->reference_line_mutex_.lock();
        int i = 0;
        for (; i < this->reference_line_smooth.size() - 1; i++) {
            ref_final.push_back(this->reference_line_smooth[i]);
            ReferencePoint add_point;
            add_point.x = (reference_line_smooth[i].x + reference_line_smooth[i+1].x) / 2;
            add_point.y = (reference_line_smooth[i].y + reference_line_smooth[i+1].y) / 2;
            add_point.heading = (reference_line_smooth[i].heading + reference_line_smooth[i+1].heading) / 2;
            add_point.kappa = (reference_line_smooth[i].kappa + reference_line_smooth[i+1].kappa) / 2;
            add_point.dkappa = (reference_line_smooth[i].dkappa + reference_line_smooth[i+1].dkappa) / 2;
            double dis = reference_line_smooth[i+1].s - reference_line_smooth[i].s;
            add_point.s = reference_line_smooth[i].s + dis / 2.0;
            ref_final.push_back(add_point);
        }
        ref_final.push_back(this->reference_line_smooth[i]);
        this->reference_line_mutex_.unlock();
    }
    
    return ref_final;
}


/**
 * 程序入口
*/
void ReferenceLineProvider::run() {

    global_path_sub_ = n.subscribe("/global_path_center", 10, &ReferenceLineProvider::global_path_callback, this);
    // TODO: 这里觉得应该使用中心位置，还得再看看博客公式推导
    rear_pose_sub_ = n.subscribe("/smart/rear_pose", 10, &ReferenceLineProvider::rear_pose_callback, this);

    reference_line_pub_ = n.advertise<nav_msgs::Path>("/reference_line", 10, true);
    boost::thread pub_ref_line_thread(boost::bind(&ReferenceLineProvider::pubReferenceLine, this));

    // ros::spin();
}